# -*- coding: utf-8 -*-
'''
Created on 25.06.2019

@author: yu03
'''

import cv2
import matplotlib.pyplot as plt
from scipy import signal
from FFT_Interpolation import FFT_interpolation_boxcar
import numpy as np

Lamda = 633e-9
pix_size = 5.3e-6
V_x, V_y, V_z = 0, 0, 0

hor_angle_centers = []
ver_angle_centers = []

hor_angle_average = []
ver_angle_average = []


for j in range(8):
    pattern_path = 'test set_2019-06-25_18-30-31/test_%i.bmp'%j
    img = cv2.imread(pattern_path, 0)
    # size = img.shape
    # print(size)
    # print(img[512][640])
    # cv2.imshow('image',img)
    # cv2.waitKey(0)
    # cv2.destroyAllWindows()
    
    hor_angle_set = []
    hor_phase_set = []
    ver_angle_set = []
    ver_phase_set = []
    
    for i in range(10):
    #     hor_center = img[512-150+i][450:850]
    #     ver_center = img[:,640-150+i][350:700]
        
        hor_center = img[431-5+i][350:750]
        ver_center = img[:,524-5+i][200:650]
            
        hor_center = hor_center / signal.gaussian(len(hor_center), std=len(hor_center)/3)
        ver_center = ver_center / signal.gaussian(len(ver_center), std=len(ver_center)/2.5)
        
        hor_center = hor_center - np.average(hor_center)
        ver_center = ver_center - np.average(ver_center)
        
        hor_freq, hor_phase = FFT_interpolation_boxcar(hor_center, pix_size)[0:2]
        ver_freq, ver_phase = FFT_interpolation_boxcar(ver_center, pix_size)[0:2]
        
        hor_angle = V_x - Lamda * hor_freq / 2
        ver_angle = V_y - Lamda * ver_freq / 2
    
        
        hor_phase = hor_phase / 2 / np.pi * 360 ### in degree
        if hor_phase < 0:
            hor_phase += 360
            
        ver_phase = ver_phase / 2 / np.pi * 360 ### in degree
        if ver_phase < 0:
            ver_phase += 360
        
#         print(hor_angle, ver_angle, hor_phase, ver_phase)
        hor_angle_set.append(hor_angle)
        hor_phase_set.append(hor_phase)
        ver_angle_set.append(ver_angle)
        ver_phase_set.append(ver_phase)
    
    hor_angle_centers.append(hor_angle_set[5])    
    hor_angle_average.append(np.average(hor_angle_set))
    ver_angle_centers.append(ver_angle_set[5])    
    ver_angle_average.append(np.average(ver_angle_set))
    
#     plt.figure(1)
#     plt.subplot(2,1,1)
#     plt.plot(hor_center)
#     plt.subplot(2,1,2)
#     plt.plot(ver_center)
#     plt.show()
    
#     plt.figure(2)
#     plt.subplot(2,2,1)
#     plt.plot(hor_angle_set)
#     plt.title("Horizontal Tilting")
#     plt.ylabel("Horizontal Tilting (rad)")
#     plt.xlabel("Horizontal Lines")
#     plt.grid(which='major', axis='both')
#      
#     plt.subplot(2,2,3)
#     plt.plot(hor_phase_set)
#     plt.title("Horizontal Phase")
#     plt.ylabel("Horizontal Phase (degree)")
#     plt.xlabel("Horizontal Lines")
#     plt.grid(which='major', axis='both')
#      
#     plt.subplot(2,2,2)
#     plt.plot(ver_angle_set)
#     plt.title("Vertical Tilting")
#     plt.ylabel("Vertical Tilting (rad)")
#     plt.xlabel("Vertical Lines")
#     plt.grid(which='major', axis='both')
#      
#     plt.subplot(2,2,4)
#     plt.plot(ver_phase_set)
#     plt.title("Vertical Phase")
#     plt.ylabel("Vertical Phase (degree)")
#     plt.xlabel("Vertical Lines")
#     plt.grid(which='major', axis='both')
#      
#     plt.tight_layout()
#     plt.show()

plt.figure(3)
plt.subplot(2,1,1)
plt.plot(hor_angle_centers, color='blue')
plt.plot(hor_angle_average, color='red')
plt.title("Horizontal Tilting")
plt.ylabel("Horizontal Tilting (rad)")
plt.xlabel("Horizontal Lines")
plt.grid(which='major', axis='both')
 
plt.subplot(2,1,2)
plt.plot(ver_angle_centers, color='blue')
plt.plot(ver_angle_average, color='red')
plt.title("Vertical Tilting")
plt.ylabel("Vertical Tilting (rad)")
plt.xlabel("Vertical Lines")
plt.grid(which='major', axis='both')
 
plt.tight_layout()
plt.show()